
#include <ctype.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <kernel.h>

#define MEM_PART_MAGIC_NUM          (0x1F23598A)
#define MEM_PART_MAGIC_FILL(p,n)    (((p) | (n)) ^ (MEM_PART_MAGIC_NUM))

#define MEM_PART_FLAG_PTRLIST       (1 << 0)
#define MEM_PART_FLAG_BOUNDPAD      (1 << 1)
#define MEM_PART_FLAG_USEMAGIC      (1 << 2)

#ifdef MEM_PART_ALIGN_CHECK
#define MEM_PART_ALIGN              0x0000FFFF
#else
#define MEM_PART_ALIGN              0x00000000
#endif

#define MEM_PART_BASE               0x00200000
#define MEM_PART_BASE_SIZE          0x00100000

#define MEM_PART_MIN_SIZE           0x00010000
#define MEM_PART_MAX_SIZE           0x01000000

#define MEM_PART_BITMAP_MIN         64

mem_part_pool * sysmempool;

static void * mem_part_malloc(mem_part_pool * mp, size_t n)
{
    int c, lr, bc, bs, pcount, tbs;
    int i, j, k, l;
    void * ret;

    lr = 0;

    /* find a suitable size for malloc */
    c = mp->bitmaplow;
    while (c < (int)n)
        {
        c = c << 1;
        lr++; /* lr is level, c is size */
        }

    /* large than max size return fail */
    if (c > mp->bitmaphigh)
        return NULL;

    /* pcount is maximum block number */

    pcount = mp->size / mp->bitmaplow;

    bs = 0;

    /* calculate: bs is how many bits before level lr,
       pcount is how many bits in level lr */

    for (i = 0; i < lr; i++)
        {
        bs = bs + pcount;
        pcount = pcount >> 1;
        }

    bc = pcount;

    /* searching in level lr and found a bit equal to 0 */

    for (i = bs; i < bc + bs; i++)
        {
        if (getbit(mp->bitmap, i) == 0)
            {
            setbit(mp->bitmap, i);
            /* found it, ret is return value, equal to its match pointer */
            ret = (char *)mp->pmem + (mp->bitmaplow << lr) * (i - bs);
            break;
            }
        }
    /* not found return fail */

    if (i >= bc + bs)
        return NULL;

    j = i - bs;

    /* set all high level(block size is larger than current) bit to be 1 */

    pcount = bc;
    tbs = pcount + bs;
    while(pcount > 0)
        {
        pcount = pcount >> 1;
        j = j >> 1;
        setbit(mp->bitmap, tbs + j);
        tbs = tbs + pcount;
        }

    j = i - bs;

    /* set all low level(block size is smaller than current) bit to be 1 */

    pcount = bc;
    pcount = pcount << 1;

    tbs = bs - pcount;
    l = 1;
    while(tbs >= 0)
        {
        j = j << 1;
        l = l << 1;
        for (k = 0; k < l; k++)
            {
            setbit(mp->bitmap, tbs + j + k);
            }
        pcount = pcount << 1;
        tbs = tbs - pcount;
        }
    return ret;
}

static void mem_part_free(mem_part_pool * mp, void * ptr, size_t n)
{
    int c, lr, bc, bs, pcount, tbs, rc;
    int i, j, k, l;

    lr = 0;
    c = mp->bitmaplow;

    /* calculate suitable size */

    while (c < (int)n)
        {
        c = c << 1;
        lr++;
        }
    /* size is too large */

    if (c > mp->bitmaphigh)
        return;

    /* rc is index bit number in current level */

    rc = ((int)ptr - (int)(mp->pmem)) / c;

    /* ptr should be align, if not, fail */
    if (((int)ptr - (int)(mp->pmem)) % c)
        return;

    /* get current level bound */

    pcount = mp->size / mp->bitmaplow;

    bs = 0;

    for (i = 0; i < lr; i++)
        {
        bs = bs + pcount;
        pcount = pcount >> 1;
        }

    bc = pcount;

    /* rc is out of bound, return fail*/

    if (rc >= bc)
        return;

    /* it has been freed? fail */

    if (getbit(mp->bitmap, bs + rc) == 0)
        return;

    /* clear it */
    clrbit(mp->bitmap, bs + rc);

    /* clear all low level bit map */

    j = rc;

    pcount = bc;
    pcount = pcount << 1;

    tbs = bs - pcount;
    l = 1;
    while(tbs >= 0)
        {
        j = j << 1;
        l = l << 1;
        for (k = 0; k < l; k++)
            {
            clrbit(mp->bitmap, tbs + j + k);
            }
        pcount = pcount << 1;
        tbs = tbs - pcount;
        }

    j = rc;
    tbs = bs;
    pcount = bc;
    /* check if its neighbour is free, if all are free, clear high level bit map*/
    while (((j % 2) == 1 && getbit(mp->bitmap, tbs + j - 1) == 0)
            || ((j % 2) == 0 && getbit(mp->bitmap, tbs + j + 1) == 0))
        {
        tbs = tbs + pcount;
        if (pcount > 0)
            {
            pcount = pcount >> 1;
            j = j >> 1;
            clrbit(mp->bitmap, tbs + j);
            }
        else
            break;
        }

    return;
}

mem_part_pool * mem_part_pool_create(void * base, size_t size, int flag, mem_part_pool * bfr)
{
    unsigned int t;
    int i, c, bsize;

    if (((int)base) & MEM_PART_ALIGN)
        return NULL;

    if (size < MEM_PART_MIN_SIZE || size > MEM_PART_MAX_SIZE)
        return NULL;

    t = size;
    c = 0;
    i = 0;

    while (t != 0)
        {
        if (t & 1)
            c++;
        t = t >> 1;
        i++;
        }

    if (c > 1)
        return NULL;

    memset(bfr, 0, sizeof(mem_part_pool));

    bfr->pmem = base;
    bfr->size = size;
    bfr->flag = flag;
    bfr->bitmap = base;
    bfr->bitmaplow = MEM_PART_BITMAP_MIN;
    bfr->bitmaphigh = (1 << (i - 2));

    bsize = size / bfr->bitmaplow / 4;

    if (bsize % MEM_PART_BITMAP_MIN)
        {
        bsize = bsize + MEM_PART_BITMAP_MIN;
        }

    bsize = bsize & (~(MEM_PART_BITMAP_MIN - 1));

    memset(bfr->bitmap, 0, bsize);

    mem_part_malloc(bfr, bsize);

    if (flag & MEM_PART_FLAG_PTRLIST)
        {
        bfr->ptrlist = mem_part_malloc(bfr, size /  bfr->bitmaplow * sizeof(int) * 2);
        }

    return bfr;
}

static void * mem_part_sys_alloc(mem_part_pool * mp, size_t nbytes)
{
    void * ptr, * ret;
    int * head;
    int paddata = sizeof(size_t);

    if (mp == NULL)
        return NULL;

    if (nbytes == 0)
        nbytes = 1;

    if (mp->flag & MEM_PART_FLAG_BOUNDPAD)
        paddata += sizeof(char *);

    if (mp->flag & MEM_PART_FLAG_USEMAGIC)
        paddata = paddata << 1;

    ptr = mem_part_malloc(mp, nbytes + paddata);

    if (ptr == NULL)
        return NULL;

    head = (int *)ptr;
    head[0] = nbytes;
    ret = &head[1];
    if (mp->flag & MEM_PART_FLAG_USEMAGIC)
        {
        head[1] = (int)MEM_PART_MAGIC_FILL((int)ptr, nbytes);
        ret = &head[2];
        }

    if (mp->flag & MEM_PART_FLAG_BOUNDPAD)
        {
        head = (int *)((char *)&head[2] + nbytes);
        head[0] = (int)ptr;
        if (mp->flag & MEM_PART_FLAG_USEMAGIC)
            {
            head[1] = (int)MEM_PART_MAGIC_FILL((int)ptr, nbytes);
            }
        }

    if (mp->flag & MEM_PART_FLAG_PTRLIST && mp->ptrlist != NULL)
        {
        int i;

        for (i = 0; i < mp->size / mp->bitmaplow * sizeof(int) * 2; i += sizeof(int) * 2)
            {
            head = (int *)((char *)mp->ptrlist + i);
            if (head[0] == (int)NULL)
                {
                head[0] = (int)ptr, head[1] = nbytes;
                break;
                }
            }
        }

    return ret;
}

static void mem_part_sys_free(mem_part_pool * mp, void * ptr)
{
    int * head, nbytes, magic1, magic2, cp, magic;
    int paddata = sizeof(size_t);

    if (mp == NULL)
        return;

    if (mp->flag & MEM_PART_FLAG_BOUNDPAD)
        paddata += sizeof(char *);

    if (mp->flag & MEM_PART_FLAG_USEMAGIC)
        paddata = paddata << 1;

    head = (int *)ptr;
    head--;
    if (mp->flag & MEM_PART_FLAG_USEMAGIC)
        {
        head--;
        }
    nbytes = head[0];
    magic1 = head[1];

    ptr = head;

    magic = MEM_PART_MAGIC_FILL((int)ptr, nbytes);

    if (mp->flag & MEM_PART_FLAG_BOUNDPAD)
        {
        head = (int *)((char *)&head[2] + nbytes);
        cp = head[0];
        if (mp->flag & MEM_PART_FLAG_USEMAGIC)
            {
            magic2 = head[1];
            if (magic2 != magic || magic1 != magic)
                return;
            }
        }

    if (mp->flag & MEM_PART_FLAG_PTRLIST && mp->ptrlist != NULL)
        {
        int i;

        for (i = 0; i < mp->size / mp->bitmaplow * sizeof(int) * 2; i += sizeof(int) * 2)
            {
            head = (int *)((char *)mp->ptrlist + i);
            if (head[0] == (int)ptr && head[1] == nbytes)
                {
                mem_part_free(mp, ptr, nbytes);
                return;
                }
            }
        }
    else
        {
        mem_part_free(mp, ptr, nbytes);
        return;
        }

    return;
}


void * malloc(size_t n)
{
    return mem_part_sys_alloc(sysmempool, n);
}

void free(void * ptr)
{
    mem_part_sys_free(sysmempool, ptr);
}

void mem_part_init()
{
    static mem_part_pool basicpool;

    sysmempool = mem_part_pool_create((int *)MEM_PART_BASE, MEM_PART_BASE_SIZE,
                                      MEM_PART_FLAG_USEMAGIC, &basicpool);

}

